/*
 * File: AltFilter.c
 *
 * Real-Time Workshop code generated for Simulink model AltFilter.
 *
 * Model version                        : 1.9
 * Real-Time Workshop file version      : 7.4  (R2009b)  29-Jun-2009
 * Real-Time Workshop file generated on : Fri Mar 12 00:19:35 2010
 * TLC version                          : 7.4 (Jul 14 2009)
 * C/C++ source code generated on       : Fri Mar 12 00:19:35 2010
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: Texas Instruments->C2000
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "AltFilter.h"
#include "AltFilter_private.h"

/* Block states (auto storage) */
D_Work_AltFilter AltFilter_DWork;

/* External inputs (root inport signals with auto storage) */
ExternalInputs_AltFilter AltFilter_U;

/* External outputs (root outports fed by signals with auto storage) */
ExternalOutputs_AltFilter AltFilter_Y;

/* Real-time model */
RT_MODEL_AltFilter AltFilter_M_;
RT_MODEL_AltFilter *AltFilter_M = &AltFilter_M_;

/* Forward declaration for local functions */
static real_T AltFilter_eml_xdotu(const real_T eml_x[2], const real_T eml_y[2]);

/* Function for Embedded MATLAB: '<Root>/AltFilter' */
static real_T AltFilter_eml_xdotu(const real_T eml_x[2], const real_T eml_y[2])
{
  return eml_x[0] * eml_y[0] + eml_x[1] * eml_y[1];
}

/* Model step function */
void AltFilter_step(void)
{
  real_T tmp[2];
  real_T unnamed_idx;
  int16_T tmp_0;
  real_T eml_I[4];
  real_T eml_I_0[4];
  real_T eml_Kk_idx;
  real_T eml_Kk_idx_0;
  static real_T tmp_1[4] = { 1.0, 0.0, 0.01, 1.0 };

  static real_T tmp_2[4] = { 6.25E-010, 1.2500000000000002E-007,
    1.2500000000000002E-007, 2.5E-005 };

  static real_T tmp_3[2] = { 1.0, 0.0 };

  /* Embedded MATLAB: '<Root>/AltFilter' incorporates:
   *  Inport: '<Root>/Zalt'
   */
  /* Embedded MATLAB Function 'AltFilter': '<S1>:1' */
  /*  X1=altitude */
  /*  X2=velocity */
  /*  0.1 m/s^2 */
  /*  2 m/s */
  /* '<S1>:1:16' */
  /*  position, velocity variances */
  /* '<S1>:1:17' */
  /*  gyro first, then accel  */
  /* '<S1>:1:1' */
  /*  calculate */
  /*  time update */
  /* '<S1>:1:27' */
  unnamed_idx = 0.0 * AltFilter_DWork.Xk[0] + AltFilter_DWork.Xk[1];
  AltFilter_DWork.Xk[0] = 0.01 * AltFilter_DWork.Xk[1] + AltFilter_DWork.Xk[0];
  AltFilter_DWork.Xk[1] = unnamed_idx;

  /* '<S1>:1:28' */
  for (tmp_0 = 0; tmp_0 < 2; tmp_0++) {
    eml_I[tmp_0] = 0.0;
    eml_I[tmp_0] += tmp_1[tmp_0] * AltFilter_DWork.Pk[0];
    eml_I[tmp_0] += tmp_1[tmp_0 + 2] * AltFilter_DWork.Pk[1];
    eml_I[tmp_0 + 2] = 0.0;
    eml_I[tmp_0 + 2] += tmp_1[tmp_0] * AltFilter_DWork.Pk[2];
    eml_I[tmp_0 + 2] += tmp_1[tmp_0 + 2] * AltFilter_DWork.Pk[3];
  }

  for (tmp_0 = 0; tmp_0 < 2; tmp_0++) {
    AltFilter_DWork.Pk[tmp_0] = (eml_I[tmp_0 + 2] * 0.01 + eml_I[tmp_0]) +
      tmp_2[tmp_0];
    AltFilter_DWork.Pk[tmp_0 + 2] = (eml_I[tmp_0] * 0.0 + eml_I[tmp_0 + 2]) +
      tmp_2[tmp_0 + 2];
  }

  /*  measurement update */
  /* '<S1>:1:32' */
  tmp[0] = 0.0;
  tmp[0] = AltFilter_DWork.Pk[0];
  tmp[0] += 0.0 * AltFilter_DWork.Pk[1];
  tmp[1] = 0.0;
  tmp[1] = AltFilter_DWork.Pk[2];
  tmp[1] += 0.0 * AltFilter_DWork.Pk[3];
  unnamed_idx = AltFilter_eml_xdotu(tmp, tmp_3) + 25.0;
  eml_Kk_idx = (AltFilter_DWork.Pk[2] * 0.0 + AltFilter_DWork.Pk[0]) /
    unnamed_idx;
  eml_Kk_idx_0 = (AltFilter_DWork.Pk[3] * 0.0 + AltFilter_DWork.Pk[1]) /
    unnamed_idx;

  /* '<S1>:1:33' */
  unnamed_idx = AltFilter_U.Zalt - AltFilter_eml_xdotu(tmp_3,
    &AltFilter_DWork.Xk[0]);
  AltFilter_DWork.Xk[0] = eml_Kk_idx * unnamed_idx + AltFilter_DWork.Xk[0];
  AltFilter_DWork.Xk[1] = eml_Kk_idx_0 * unnamed_idx + AltFilter_DWork.Xk[1];

  /* '<S1>:1:34' */
  eml_I[0] = 1.0 - eml_Kk_idx;
  eml_I[2] = 0.0 - eml_Kk_idx * 0.0;
  eml_I[1] = 0.0 - eml_Kk_idx_0;
  eml_I[3] = 1.0 - eml_Kk_idx_0 * 0.0;
  for (tmp_0 = 0; tmp_0 < 2; tmp_0++) {
    eml_I_0[tmp_0] = 0.0;
    eml_I_0[tmp_0] += eml_I[tmp_0] * AltFilter_DWork.Pk[0];
    eml_I_0[tmp_0] += eml_I[tmp_0 + 2] * AltFilter_DWork.Pk[1];
    eml_I_0[tmp_0 + 2] = 0.0;
    eml_I_0[tmp_0 + 2] += eml_I[tmp_0] * AltFilter_DWork.Pk[2];
    eml_I_0[tmp_0 + 2] += eml_I[tmp_0 + 2] * AltFilter_DWork.Pk[3];
  }

  AltFilter_DWork.Pk[0] = eml_I_0[0];
  AltFilter_DWork.Pk[1] = eml_I_0[1];
  AltFilter_DWork.Pk[2] = eml_I_0[2];
  AltFilter_DWork.Pk[3] = eml_I_0[3];

  /* '<S1>:1:35' */
  /* '<S1>:1:36' */

  /* Outport: '<Root>/alt' */
  AltFilter_Y.alt = AltFilter_DWork.Xk[0];

  /* Outport: '<Root>/velocity' */
  AltFilter_Y.velocity = AltFilter_DWork.Xk[1];
}

/* Model initialize function */
void AltFilter_initialize(void)
{
  /* Registration code */

  /* initialize error status */
  rtmSetErrorStatus(AltFilter_M, (NULL));

  /* states (dwork) */
  (void) memset((void *)&AltFilter_DWork, 0,
                sizeof(D_Work_AltFilter));

  /* external inputs */
  AltFilter_U.Zalt = 0.0;

  /* external outputs */
  (void) memset((void *)&AltFilter_Y, 0,
                sizeof(ExternalOutputs_AltFilter));

  /* InitializeConditions for Embedded MATLAB: '<Root>/AltFilter' */
  AltFilter_DWork.Xk[0] = 0.0;
  AltFilter_DWork.Xk[1] = 0.0;
  AltFilter_DWork.Pk[0] = 6.25E-010;
  AltFilter_DWork.Pk[1] = 1.2500000000000002E-007;
  AltFilter_DWork.Pk[2] = 1.2500000000000002E-007;
  AltFilter_DWork.Pk[3] = 2.5E-005;
}

/* Model terminate function */
void AltFilter_terminate(void)
{
  /* (no terminate code required) */
}

/*
 * File trailer for Real-Time Workshop generated code.
 *
 * [EOF]
 */
